#!/usr/bin/env python
#encoding=utf-8

import sys
from look_at_bin import look_at_bin
import rospy
import actionlib
from control_msgs.msg import (FollowJointTrajectoryAction,
                              FollowJointTrajectoryGoal,
                              GripperCommandAction,
                              GripperCommandGoal)
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

def main():
    #夹取器
    gripper_joint_names = ["l_gripper_finger_joint","r_gripper_finger_joint"]
    gripper_positions = [0.01,0.03,0.05,0.06,0.08]

    #手臂
    arm_joint_names = ["torso_lift_joint","shoulder_pan_joint", "shoulder_lift_joint", # elbow_flex_joint 2;  wrist_flex_joint 3
                        "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", # shoulder_lift_joint 1
                        "wrist_flex_joint", "wrist_roll_joint"]
    arm_joint_positions  = [[0.5,0.0,-0.8,0.0,0.0,0.0,1.42,1.57], [0.5,0.0,-0.70,0.0,0.85,0.0,1.42,1.57]]

    #等待服务连接
    rospy.loginfo("Waiting for arm_with_torso_controller...")
    arm_client = actionlib.SimpleActionClient("arm_with_torso_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
    arm_client.wait_for_server()
    rospy.loginfo("...connected.")

    rospy.loginfo("Waiting for gripper_controller...")
    gripper_client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
    gripper_client.wait_for_server()
    rospy.loginfo("...connected.")

    #机械臂姿态
    trajectory = {}
    trajectory = JointTrajectory()
    trajectory.joint_names = arm_joint_names
    for i in range(len(arm_joint_positions)):	
        trajectory.points.append(JointTrajectoryPoint())
        trajectory.points[i].positions = arm_joint_positions[i]
        trajectory.points[i].velocities =  [0.0] * len(arm_joint_positions[i])
        trajectory.points[i].accelerations = [0.0] * len(arm_joint_positions[i])
        trajectory.points[i].time_from_start = rospy.Duration(1.0 + 2.0 * i)
    arm_goal = FollowJointTrajectoryGoal()
    arm_goal.trajectory = trajectory
    arm_goal.goal_time_tolerance = rospy.Duration(0.0)
    #夹取器姿态
    gripper_goal = GripperCommandGoal()
    gripper_goal.command.max_effort = 10.0
    gripper_goal.command.position = gripper_positions[4]

    #发送goal
    gripper_client.send_goal(gripper_goal)
    gripper_client.wait_for_result(rospy.Duration(0.0))
    arm_client.send_goal(arm_goal)
    arm_client.wait_for_result(rospy.Duration(0.0))
    look_at_bin()

if __name__ == "__main__":
    rospy.init_node("fetch_prepare")
    main()



